#include <unistd.h>
#include <iostream>
#include "PCA9535.h"

#include "wiringPi.h"
#include "wiringPiI2C.h"

PCA9535::PCA9535(int addr)
{
		mAddr = addr;
		mFd = -1;
}
PCA9535::~PCA9535()
{
		if(mFd > 0)
		{
				close(mFd);
				mFd = -1;
		}
}
bool PCA9535::begin()
{
		mFd = wiringPiI2CSetup(mAddr);
		std::cout << "PCA9535 addr:" << mAddr << ",fd = " << mFd << std::endl;
		if(mFd < 0)
		{
				return false;
		}
	return true;
}
void PCA9535::setAddr(int addr)
{
		mAddr = addr;
}

int PCA9535::readInPort(int id)
{
		int val = 0;
		if(mFd > 0)
		{
			val = wiringPiI2CReadReg8(mFd,(id==0)?0x00:0x01);
		}
		mInPort[id] = val;
		return val;
}
void PCA9535::setOutPort(int id,int val)
{
		if(mFd > 0)
		{
				wiringPiI2CWriteReg8(mFd,(id==0)?0x02:0x03,val);
				mOutPort[id] = val;
		}
}

void PCA9535::setConfigPort(int id,int val)
{
		if(mFd > 0)
		{
				wiringPiI2CWriteReg8(mFd,(id==0)?0x06:0x07,val);
				mConfigPort[id] = val;
		}
}

void PCA9535::pinMode(int pin,int mode)
{
  int mask, old, reg ;
  int group = pin/ 8;
  int id = pin % 8;

  reg  = (group == 0)? 0x06: 0x07;
  mask = 1 << id;
  old  = wiringPiI2CReadReg8 (mFd, reg) ;

  if (mode == OUTPUT)
    old &= (~mask) ;
  else
    old |=   mask ;

  wiringPiI2CWriteReg8 (mFd, reg, old) ;

  mConfigPort[group] = old; // update
}

void PCA9535::digitalWrite(int pin,int val)
{
  int bit, old ;
  int group = pin / 8;
  int id = pin % 8;
  int reg = (group==0)? 0x02: 0x03;

  bit  = 1 << (id) ;

  old  = wiringPiI2CReadReg8 (mFd, reg) ;

  if (val == LOW)
    old &= (~bit) ;
  else
    old |=   bit ;

  wiringPiI2CWriteReg8 (mFd, reg, old) ;

  mOutPort[group] = old; // update
}

int PCA9535::digitalRead(int pin)
{
  int mask, value ;
  int group = pin / 8;
  int id = pin % 8;
  int reg = (group==0)? 0x00: 0x01;

  mask  = 1 << (id & 7) ;
  value = wiringPiI2CReadReg8 (mFd, reg) ;

  if ((value & mask) == 0)
    return LOW ;
  else 
    return HIGH ;
}


